#include "MMC5983MA.h"
#include <math.h>

MMC5983MA::MMC5983MA() : _wire(nullptr), _addr(MMC5983MA_ADDR) {
}

MMC5983MA::~MMC5983MA() {
}

bool MMC5983MA::begin(TwoWire *wire, uint8_t addr) {
    _wire = wire;
    _addr = addr;
    
    // Initialize I2C
    _wire->begin();
    
    // Check if the sensor is connected
    if (!isConnected()) {
        return false;
    }
    
    // Reset the sensor
    writeRegister(MMC5983MA_CONTROL_0, MMC5983MA_CONTROL0_SRST);
    delay(10);
    
    // Set/Reset procedure
    setReset();
    delay(10);
    setSet();
    delay(10);
    
    return true;
}

bool MMC5983MA::isConnected() {
    _wire->beginTransmission(_addr);
    return (_wire->endTransmission() == 0);
}

void MMC5983MA::startSingleMeasurement() {
    // Trigger single measurement
    writeRegister(MMC5983MA_CONTROL_0, MMC5983MA_CONTROL0_TM_M);
}

bool MMC5983MA::isDataReady() {
    return (readRegister(MMC5983MA_STATUS) & MMC5983MA_STATUS_DRDY);
}

void MMC5983MA::readMagneticField(int16_t *x, int16_t *y, int16_t *z) {
    // Start a single measurement if not in continuous mode
    startSingleMeasurement();
    
    // Wait for data ready
    while (!isDataReady()) {
        delay(1);
    }
    
    // Read all 6 bytes of magnetic data
    uint8_t data[6];
    readRegisters(MMC5983MA_XOUT0, data, 6);
    
    // Combine 2 bytes for each axis and mask with 18 bits
    *x = ((int16_t)(data[1] << 8 | data[0])) & 0x3FFF;
    *y = ((int16_t)(data[3] << 8 | data[2])) & 0x3FFF;
    *z = ((int16_t)(data[5] << 8 | data[4])) & 0x3FFF;
    
    // Convert to signed values
    if (*x & 0x2000) *x |= 0xC000;
    if (*y & 0x2000) *y |= 0xC000;
    if (*z & 0x2000) *z |= 0xC000;
}

void MMC5983MA::readMagneticField(float *x, float *y, float *z) {
    int16_t xi, yi, zi;
    readMagneticField(&xi, &yi, &zi);
    
    // Convert to Gauss (typical sensitivity is ~0.8 mG/LSB)
    *x = xi * 0.0008f;
    *y = yi * 0.0008f;
    *z = zi * 0.0008f;
}

float MMC5983MA::readTemperature() {
    // Start temperature measurement
    writeRegister(MMC5983MA_CONTROL_0, MMC5983MA_CONTROL0_TM_T);
    
    // Wait for measurement to complete
    delay(5);
    
    // Read temperature value
    uint8_t temp = readRegister(MMC5983MA_TOUT);
    
    // Convert to Celsius (typical formula: T = temp * 0.8 - 75)
    return (temp * 0.8f) - 75.0f;
}

void MMC5983MA::setReset() {
    uint8_t control1 = readRegister(MMC5983MA_CONTROL_1);
    writeRegister(MMC5983MA_CONTROL_1, control1 | MMC5983MA_CONTROL1_RESET);
}

void MMC5983MA::setSet() {
    uint8_t control1 = readRegister(MMC5983MA_CONTROL_1);
    writeRegister(MMC5983MA_CONTROL_1, control1 | MMC5983MA_CONTROL1_SET);
}

void MMC5983MA::setAutoMeasurement(bool enable) {
    uint8_t control1 = readRegister(MMC5983MA_CONTROL_1);
    if (enable) {
        control1 |= MMC5983MA_CONTROL1_AUTO_SR_EN;
    } else {
        control1 &= ~MMC5983MA_CONTROL1_AUTO_SR_EN;
    }
    writeRegister(MMC5983MA_CONTROL_1, control1);
}

void MMC5983MA::cal() {
    // Set the sensor in calibration mode
    // First set/reset the sensor
    setReset();
    delay(10);
    setSet();
    delay(10);
    
    // Example calibration routine - actual calibration may need more sophisticated approach
    // This is a placeholder implementation that can be expanded
    
    // Perform a set of measurements to calibrate
    uint8_t control1 = readRegister(MMC5983MA_CONTROL_1);
    // Enable continuous measurement mode during calibration
    control1 |= MMC5983MA_CONTROL1_EN_CMM;
    writeRegister(MMC5983MA_CONTROL_1, control1);
    
    // Start continuous measurement
    writeRegister(MMC5983MA_CONTROL_0, MMC5983MA_CONTROL0_TM_M);
    
    // Wait a moment for calibration to stabilize
    delay(100);
    
    // After calibration, disable continuous measurement mode
    control1 = readRegister(MMC5983MA_CONTROL_1);
    control1 &= ~MMC5983MA_CONTROL1_EN_CMM;
    writeRegister(MMC5983MA_CONTROL_1, control1);
}

void MMC5983MA::peeling() {
    // This method is used to remove the current magnetic field environment
    // It typically involves measuring the current magnetic field and storing
    // it as an offset to be subtracted from future measurements
    
    // First set/reset the sensor
    setReset();
    delay(10);
    setSet();
    delay(10);
    
    // Start a single measurement
    startSingleMeasurement();
    
    // Wait for data ready
    while (!isDataReady()) {
        delay(1);
    }
    
    // Read the current magnetic field values
    // In a real implementation, these values would be stored as offsets
    // and subtracted from future measurements
    int16_t x, y, z;
    readMagneticField(&x, &y, &z);
    
    // Enable offset compensation mode (example implementation)
    // For MMC5983MA, this typically involves setting specific register bits
    // or using a dedicated calibration register
    uint8_t control2 = readRegister(MMC5983MA_CONTROL_2);
    // Set appropriate bits for offset compensation if available
    // This is a placeholder implementation
    writeRegister(MMC5983MA_CONTROL_2, control2);
}

float MMC5983MA::get_heading() {
    // Read magnetic field values
    float x, y, z;
    readMagneticField(&x, &y, &z);
    
    // Calculate heading using atan2 function
    // The formula is: heading = arctangent(y, x)
    // This gives angle in radians
    float heading = atan2(y, x);
    
    // Convert from radians to degrees
    heading = heading * 180.0 / M_PI;
    
    // Normalize to 0-360 degrees
    if (heading < 0) {
        heading += 360.0;
    }
    
    return heading;
}

void MMC5983MA::writeRegister(uint8_t reg, uint8_t value) {
    _wire->beginTransmission(_addr);
    _wire->write(reg);
    _wire->write(value);
    _wire->endTransmission();
}

uint8_t MMC5983MA::readRegister(uint8_t reg) {
    _wire->beginTransmission(_addr);
    _wire->write(reg);
    _wire->endTransmission();
    
    _wire->requestFrom(_addr, (uint8_t)1);
    if (_wire->available()) {
        return _wire->read();
    }
    return 0;
}

void MMC5983MA::readRegisters(uint8_t reg, uint8_t *data, size_t len) {
    _wire->beginTransmission(_addr);
    _wire->write(reg);
    _wire->endTransmission();
    
    _wire->requestFrom(_addr, (uint8_t)len);
    size_t i = 0;
    while (_wire->available() && i < len) {
        data[i++] = _wire->read();
    }
}